package ssi.staaar.util;

import android.util.Log;

public class GeoUtils {
	private static final double cameraHorizontalAngle = 41.1;
	private static final double cameraVerticalAngle = 53.6;
	
	public static double bearing(double lat1, double lon1, double lat2, double lon2) {
        double lat1Rad = Math.toRadians(lat1);
        double lat2Rad = Math.toRadians(lat2);
        double deltaLonRad = Math.toRadians(lon2 - lon1);

        double y = Math.sin(deltaLonRad) * Math.cos(lat2Rad);
        double x = Math.cos(lat1Rad) * Math.sin(lat2Rad) - Math.sin(lat1Rad) * Math.cos(lat2Rad)
                * Math.cos(deltaLonRad);
       
        return radToBearing(Math.atan2(y, x));
    }
    
    public static double radToBearing(double rad) {
        return (Math.toDegrees(rad) + 360) % 360;
    }
    
    public static double normalizeDegree(double degrees) {
    	return ((degrees + 360) % 360);
    }
    
    public static int getHorizontalPosition(double lat, double lon,
			double currentLat, double currentLon, double azimuth, int screenWidth) {
		double left_arm = normalizeDegree(azimuth - cameraHorizontalAngle/2);
		double right_arm = normalizeDegree(azimuth + cameraHorizontalAngle/2);
		double palloccoBearing = (float)bearing(currentLat, currentLon, lat, lon);
	
		if(right_arm < left_arm){
			if(palloccoBearing < right_arm){
				palloccoBearing = palloccoBearing + 360;
			}
			right_arm = right_arm + 360;
		}
		
			double angleFromLeftArm = palloccoBearing - left_arm;
			float x_value = (float)((angleFromLeftArm/cameraHorizontalAngle)*screenWidth);
			int x = Math.round(x_value);
			return x;
		}
    
	public static int getVerticalPosition(double alt, double dist,
			double currentAlt, double inclination, int screenHeight) {
		inclination = -inclination;
		Log.d("[AR]", "inclination: " + inclination);
		double upper_arm = inclination + cameraVerticalAngle/2;
		double lower_arm = inclination - cameraVerticalAngle/2;
		Log.d("[AR]", "upper_arm: " + upper_arm);
		Log.d("[AR]", "lower_arm: " + lower_arm);
		double mDist;
		if (dist == 0.0)
			mDist = (dist+0.1)*1000;
		else 
			mDist = dist*1000;
		Log.d("[AR]", "distance m: " + mDist);
		double messageAngle = Math.toDegrees(Math.atan((alt-currentAlt)/mDist));
		Log.d("[AR]", "message angle: " + messageAngle);
		//calcolare dove si trova il punto rispetto a me
		double result = upper_arm - messageAngle;
		Log.d("[AR]", "distanza del mes dall'upper: " + result);
		
		double y_value = (result/cameraVerticalAngle)*screenHeight;
		int y = (int) Math.round(y_value);
		Log.d("[AR]", "y: " + y);
		return y;
	}

}
